clc
clear
%%
%地球参数设置
v_ie=7.292e-5; %地球自转角速率
a=6378254;     %地球长半径
e=1/298.3;     %偏心率
g=9.8;

%
%p_new=[0;0;0];
R_mh=R_m+h_new;
R_nh=R_n+h_new;

p_new(1)=L_new;
p_new(2)=la_new;
p_new(3)=h_new;

p_before(1)=L_before;
p_before(2)=la_before;
p_before(3)=h_before;

%地球卯西圈和子午圈半径
R_m=a/sqrt(1-e^2*sin(L_new));
R_n=a*(1-e^2)/sqrt((1-e^2*sin(L_new)^2)^3);

%%
%开始姿态更新

%姿态更新方程
c_new=c_n*c_before*c_b;
%第一个矩阵


w_ie=[0;v_ie*cos(L_new);v_ie*sin(L_new)];
w_en=[-v_n/R_mh;v_e/R_nh;v_e/R_nh*tan(L_new)];

w_in=w_ie+w_en;
erv_in=ts*w_in;

c_n=mrv(erv_in);
c_n=c_n';
%第二个矩阵
up_m1=angle_ts_half-angle_ts_begin;
up_m2=angle_ts_end-angle_ts_half;
up_m_sum=up_m1+up_m2;
erv_ib=(up_m1+up_m2)+cross(2/3*up_m1,up_m2);

c_b=mrv(erv_ib);
%%
%开始速度更新

erv_in2=1/2*erv_in;
c_n2=mrv(erv_in2);

up_vm_sum=up_vm1+up_vm2;
up_vm1=vel_ts_half-vel_ts_begin;
up_vm2=vel_ts_end-vel_ts_half;

up_v_rotm=1/2*cross(up_m_sum,up_vm_sum);
up_v_scullm=2/3*(cross(up_m1,up_vm2)+cross(up_vm1,up_m2));

up_sf=up_vm_sum+up_v_rotm+up_v_scullm;

w_ie_half=(3*w_ie_before-w_ie_before2)/2;
w_en_half=(3*w_en_before-w_en_before2)/2;
v_half=(3*v_before-v_before2)/2;
up_g=(g-cross((2*w_ie_half+w_en_half),v_half))*ts;

a_m=(c_n2*c_before*up_sf+up_g)/ts;

v_new=v_before+a_m*ts;
%%
%开始位置更新

h_out=(3*h_before-h_before2)/2;
R_mh_out=R_m+h_out;
R_nh_out=R_n+h_out;

L_out=(3*L_before-L_before2)/2;

mpv=[0,1/R_mh_out,0;sec(L_out)/R_nh_out,0,0;0,0,1];

v_ave=(v_new+v_before)/2;

p_new=p_before+mpv*v_ave*ts;

